
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "config.h"
#include "util.h"

static void FindChilds(int node, int childs[][2])
{
    int x, y, i;
    x = node % MAP_COLS;
    y = node / MAP_COLS;

    for(i = 0; i < 8; i++){
        childs[i][0] = -1;
        childs[i][1] = -1;
    }

    if(x-1 >= 0 && y - 1 >= 0) {
        childs[0][0] = x - 1;
        childs[0][1] = y - 1;
    }
    if(x - 1 >= 0 && y + 1 < MAP_ROWS) {
        childs[1][0] = x - 1;
        childs[1][1] = y + 1;
    }
    if(x + 1 < MAP_COLS && y - 1 >= 0) {
        childs[2][0] = x + 1;
        childs[2][1] = y - 1;
    }
    if(x + 1 < MAP_COLS && y + 1 < MAP_ROWS) {
        childs[3][0] = x + 1;
        childs[3][1] = y + 1;
    }
    if(x + 1 < MAP_COLS){
        childs[4][0] = x + 1;
        childs[4][1] = y;
    }
    if(x - 1 >= 0) {
        childs[5][0] = x - 1;
        childs[5][1] = y;
    }
    if(y + 1 < MAP_ROWS) {
        childs[6][0] = x;
        childs[6][1] = y + 1;
    }
    if(y - 1 >= 0) {
        childs[7][0] = x;
        childs[7][1] = y - 1;
    }
}

boolean LocalOptimalFindPath(int from, int to, int map[], int path[])
{
    if(from < 0 || from >= MAP_COLS * MAP_ROWS ||
       to < 0 || to >= MAP_COLS * MAP_ROWS)
        return false;

    if(map[to] != FLOOR_GLYPH) return false;

    printf("from: %d; to: %d\n", from, to);

    int neighbors[8][2];
    int x, y, i, grid_id, current_grid;
    int goal_x, goal_y;
    int next_x, next_y;
    int old_cost = 0, estimate_cost;
    int path_point = 0;
    char explored[MAP_COLS* MAP_ROWS];
    int max_path_point = MAP_COLS * MAP_ROWS;

    memset(explored, 0, sizeof explored);
    for(i = 0; i < max_path_point; ++ i)
        path[i] = -1;

    x = from % MAP_COLS;
    y = from / MAP_COLS;
    goal_x = to % MAP_COLS;
    goal_y = to / MAP_COLS;

    while(true) {
        current_grid = MAP_COLS * y + x;
        explored[current_grid] = 1;

        FindChilds(current_grid, neighbors);

        next_x = -1;
        next_y = -1;
        estimate_cost = -1;

        for(i = 0; i < 8; i++) {
            grid_id = neighbors[i][0] + neighbors[i][1] * MAP_COLS;
            if(neighbors[i][0] < 0 || // outside map
               map[grid_id] != FLOOR_GLYPH ||  // block
               explored[grid_id] == 1)
                continue;
            int new_cost = old_cost +
                1 +
                /* abs(neighbors[i][0] - x) + abs(neighbors[i][1] - y) + */
                abs(goal_x - neighbors[i][0]) + abs(goal_y - neighbors[i][1]);
            if (estimate_cost < 0 || new_cost < estimate_cost){
                estimate_cost = new_cost;
                next_x = neighbors[i][0];
                next_y = neighbors[i][1];
            }
        }

        if (next_x >= 0 && next_y >= 0){
            path[path_point ++] = next_x + next_y * MAP_COLS;
            /* old_cost += abs(next_x - x) + abs(next_y - y); */
            old_cost += 1;

            if (next_x == goal_x && next_y == goal_y) {
                printf("path: ");
                for(i =0; i< path_point; i++)
                    printf("%d ", path[i]);
                printf("\n");
                return true;
            }

            if (path_point >= max_path_point)
                return false;

            x = next_x;
            y = next_y;
        }
        else // no neighbor grid to explore
            return false;
    }
}

/* If a given node is in open list, return its index,
   Otherwise return -1. */
static int NodeInOpenList(int node, int open_list[], 
                              int open_node_count)
{
    int i;
    for(i = 0; i < open_node_count; i++){
        if(open_list[i] == node)
            return i;
    }
    return -1;
}

/* Remove open node from open list */
static void RmOpenNode(int node, int open_list[], int *open_node_count)
{
    int i, index;

    index = NodeInOpenList(node, open_list, *open_node_count);
    if(index < 0) return;

    for(i = index; i < *open_node_count - 1; i ++){
        open_list[i] = open_list[i+1];
    }
    open_list[*open_node_count - 1] = -1;
    *open_node_count -= 1;
}

static void AddOpenNode(int node, int parent, int fn,
                 int open_list[], int *open_node_count,
                 int (*open_node_info)[2])
{
    /* If there are already too many open nodes */
    if(*open_node_count >= MAP_COLS * MAP_ROWS)
        return;

    /* Suppose the node is not in open_list  */
    open_list[*open_node_count] = node;
    (*open_node_count) ++;
    /* Suppose node and parent are both valid */
    open_node_info[node][0] = fn;
    open_node_info[node][1] = parent;
}

/* return the minimum f(n) of open_list,
   At the same time, return the node with min fn through param */
static int MinFnOpenList(int open_list[], int open_node_count,
                         int open_node_info[][2], int *min_node)
{
    int i, node, min_fn, min_fn_node;
    min_fn = -1;
    min_fn_node = -1;
    for(i = 0; i < open_node_count; i++){
        node = open_list[i];
        if(min_fn < 0 || min_fn > open_node_info[node][0]){
            min_fn = open_node_info[node][0];
            min_fn_node = node;
        }
    }
    *min_node = min_fn_node;
    return min_fn;
}

static void InitOpenList(int open_list[], int *open_node_count,
                         int (*open_node_info)[2])
{
    int i;
    for(i = 0; i < MAP_COLS * MAP_ROWS; i ++){
        open_list[i] = -1;
        open_node_info[i][0] = -1;
        open_node_info[i][1] = -1;
    }
    *open_node_count = 0;
}

static int NodeHn(int node, int goal)
{
    int x1, x2, y1, y2;
    x1 = node % MAP_COLS;
    y1 = node / MAP_COLS;
    x2 = goal % MAP_COLS;
    y2 = goal / MAP_COLS;
    return abs(x1 - x2) + abs(y1 - y2);
}

static void TracePath(int node, int open_node_info[][2], int path[])
{
    int i;
    int count = 0;

    /* Clear path */
    for(i = 0; i < MAP_COLS * MAP_ROWS; i++)
        path[i] = -1;

    i = node;
    while(open_node_info[i][1] > 0){ /* If node has parent */
        count ++;
        i = open_node_info[i][1];
    }

    if(count == 0) return;

    path[count - 1] = node;
    for(i = count - 2; i >= 0; i--){
        path[i] = open_node_info[path[i + 1]][1];
    }
}

boolean AstarFindPath(int from, int to, int map[], int path[])
{
    if(from < 0 || from >= MAP_COLS * MAP_ROWS ||
       to < 0 || to >= MAP_COLS * MAP_ROWS)
        return false;

    if(map[to] != FLOOR_GLYPH) return false;

    int open_list[MAP_COLS * MAP_ROWS];
    int open_node_count;
    int open_node_info[MAP_COLS * MAP_ROWS][2]; /* 0: fn, 1: father */

    char explored[MAP_COLS * MAP_ROWS]; /* closed list */

    int cost[MAP_COLS * MAP_ROWS];

    int min_fn, min_fn_node, node_fn;
    int neighbors[8][2];
    int i, x, y;
    int current_node, child_node;

    memset(explored, 0, MAP_COLS * MAP_ROWS);
    InitOpenList(open_list, &open_node_count, open_node_info);
    cost[from] = 0;
    node_fn = NodeHn(from, to);
    AddOpenNode(from, -1, node_fn, open_list, &open_node_count, open_node_info);

    while(open_node_count > 0) {
        /* cost update */
        min_fn = MinFnOpenList(open_list, open_node_count, open_node_info,
                               &min_fn_node);
        current_node = min_fn_node;
        if(current_node == to) break;

        RmOpenNode(current_node, open_list, &open_node_count);
        explored[current_node] = 1;

        min_fn = MinFnOpenList(open_list, open_node_count, open_node_info,
                               &min_fn_node);
        FindChilds(current_node, neighbors);
        for(i = 0; i < 8; i++){
            child_node = neighbors[i][0] + neighbors[i][1] * MAP_COLS;
            if(explored[child_node] || map[child_node] != FLOOR_GLYPH
               || neighbors[i][0] == -1)
                continue;
            cost[child_node] = cost[current_node] + 1;
            node_fn = cost[child_node] + NodeHn(child_node, to);
            if(/* (node_fn <= min_fn || min_fn < 0) && */
               NodeInOpenList(child_node, open_list, open_node_count == -1)){
                AddOpenNode(child_node, current_node, node_fn, open_list,
                            &open_node_count, open_node_info);
            }
        } // end for each child
    } // end while open list not empty

    /* If find path success */
    if(current_node == to) {
        TracePath(to, open_node_info, path);
        return true;
    }

    return false;
}
